00100	IFN USER+TRACK,<0>
00200	STRADR:	MOVE BITSAV	;Enters here every 1/60 th. seccond
00300	IFE USER+TRACK,<
00400		TRNE DTST
00500		CALLI 400024
00600		DATAO ARM,[2]
00700		SOSLE STWT
00800		CALLI 400024
00900	>
01000		MOVE P,[XWD -PDLL,PDL]	;PUSH DOWN STACK
01100		MOVE CBUF,CBUFS
01200		MOVE DATWD,DATWDS
01300		IORI DATWD,2		;YELLOW ARM
01400	IFN REPORT+TRACK,<
01500		MOVE DATA,DATPT
01600	>
01700	IFN USER+TRACK,<
01800		TRNE DTST
01900		JRST SWDONE
02000	>
02100	IFE USER,<
02200	IFN TRACK,<
02300		SKIPE WALK
02400		JRST NCHK
02500	>
02600		CONSZ 40		;CHECK TO SEE IF ON PDP-6
02700		JRST[	MOVEI TAC,PDPERR
02800			MOVEM TAC,TRAJER
02900			JRST TOFF]
03000	;	CONO 4,400		;TURN OFF THE PI'S
03100		CONI 0,AC		;SET PROCESSOR FLAGS
03200		ANDI AC,7
03300		CONO 0,2050(AC)
03400		TRZE TRIP		;TURN ON ARM
03500		DATAO ARM,[3]
03600	>
03700	NCHK:
03800		MOVEI K,51B23		;-10 VOLT REFERENCE
03900	IFE USER,<PUSHJ P,PREAD		;REFTR← -10 volt reference
04000		LDB AC,SNUM		;PICK UP MIDDLE SAMPLE
04100		ANDI DACVAL,7777	;LEAVE LAST SAMPLE IN J
04200		ADDI AC,(DACVAL)	;ADD THEM TOGETHER
04300	>
04400	IFN USER,<MOVEI AC,17770>
04500		CAIGE AC,17000		;IS THE FUSE BLOWN
04600		JRST[		MOVEI TAC,REFERR
04700				MOVEM TAC,TRAJER
04800				JRST TOFF]
04900		MOVEM AC,REFTR		;REFERENCE READING
05000	IFN TRACK,<
05100		SKIPE WALK
05200		JRST TOK
05300	>
05400	IFE USER,<
05500		CONI CLOCK,AC
05600		TLZ AC,777774
05700		JRST STIME		;TIME IN MICRO SECONDS
05800	>
05900	IFN USER,<JRST .+2>
     

00100	NXTJIF:
00200	IFN USER,<JRST SWDONE>
00300	IFN DEB<
00400		MOVEM 17,ACSAV+17
00500		MOVEI 17,ACSAV
00600		BLT 17,ACSAV+16
00700		MOVE 17,ACSAV+17
00800	>
00900	IFN TRACK,<
01000		SKIPE WALK
01100		JRST TOK
01200	>
01300	IFN REPORT+TRACK,<
01400		HRRZI TAC,(DATA)
01500		CAIL TAC,DATO+DBL-1
01600		JRST[	SUB DATA,[XWD DBL,DBL]
01700			MOVE TAC,[XWD DATO+DBL,DATO]
01800			BLT TAC,(DATA)
01900			HRRZI AC,(DATA)
02000			HRRZ TAC,DATIWD
02100			SUBI AC,(TAC)
02200			JRST RDP]
02300		HRRZI AC,(DATA)
02400		HRRZ TAC,DATIWD
02500		SUBI AC,(TAC)
02600		HRRZ MQ,DATPT
02700		SUBI TAC,(MQ)
02800		IMUL AC,TAC
02900	RDP:	SKIPLE AC
03000	IFN TRACK,<PUSH DATA,[0]>
03100	IFN REPORT,<PUSH DATA,[SIXBIT/ERROR/]>
03200		MOVEI AC,DATO-1
03300		HRRZ TAC,DATIWD
03400	 	CAIL TAC,DATO+DBL-1
03500		HRRM AC,DATIWD
03600		MOVEM DATA,DATPT
03700	>
03800	IFE USER,<
03900		MOVEI TAC,=2000
     

00100	WATSTL:	CONI CLOCK,AC
00200		TLZ AC,777774
00300		SUB AC,LTIME
00400		JUMPGE AC,.+2
00500		ADD AC,[=1000000]
00600	IFN REPORT,<
00700		CAIN TAC,=2000
00800		JRST[	PUSH DATA,[<SIXBIT/TICK/>+1]
00900			HRRZ MQ,DATIWD
01000			PUSH DATA,MQ
01100			JRST .+1]
01200	>
01300		MOVEM AC,ETIME#
01400		TRNE RUN
01500		JRST[	SKIPE TP
01600			JRST .+1
01700			MOVE MQ,AC
01800			FSC MQ,233
01900			FMPR MQ,[0.00006]
02000			MOVEM MQ,TDF#
02100			MOVE AC,LTIME
02200			ADD AC,ETIME
02300			JRST SSTIME]
02400		CAIGE AC,=16667		;delta T greater than 16.7 msec.?
02500		SOJGE TAC,WATSTL
02600		JUMPL TAC,TE
02700		CAIL AC,=19000
02800		JRST[	MOVEI TAC,ISERR
02900			MOVEM TAC,TRAJER
03000			JRST TOFF]
03100		MOVSI AC,(1.0)
03200		MOVEM AC,TDF
03300		MOVE AC,LTIME
03400		ADDI AC,=16667
03500	SSTIME:	TLNE AC,777774
03600		SUB AC,[=1000000]
03700	STIME:	MOVEM AC,LTIME		;LTIME←LTIME+16.7 msec.
03800	>
03900		SOSGE COUNT
04000		JRST[TE:MOVEI AC,TERR
04100			MOVEM AC,TRAJER
04200			JRST ZITO]
04300	IFN ADJUST,<
04400		MOVEI K,14B23
04500		MOVEM BITSAV
04600		MOVEI I,2
04700	RNP:	PUSHJ P,PREAD
04800		LDB AC,SNUM
04900		FSC AC,221
05000		JSA 16,EXP
05100		JUMP AC
05200		FMPR [0.15]
05300		MOVEM FACTOR(I)
05400		SOJGE I,RNP
05500		MOVE BITSAV
05600		MOVE I,ADJI
05700		JUMPL I,MAIN
05800	IFN ADJG,<
05900		MOVE AC,ADJE
06000		FMPR AC,FACTOR
06100		MOVEM AC,KE(I)		;POT 12
06200		MOVE AC,ADJV
06300		FMPR AC,FACTOR+1
06400		MOVEM AC,KV(I)		;POT 13
06500		MOVE AC,ADJIF
06600		FMPR AC,FACTOR+2
06700		MOVEM AC,KI(I)	;POT 14
06800	>
06900	IFN ADJD,<
07000		MOVE AC,ADJE
07100		FMPR AC,FACTOR
07200		MOVEM AC,KM(I)		;POT 12
07300		MOVE AC,ADJV
07400		FMPR AC,FACTOR+1
07500		MOVEM AC,PK(I)		;POT 13
07600	>
07700	>
     

00100	MAIN:	PUSHJ P,ANGLES		;read joint angles
00200		TRNE RONLY		;read angles only?
00300		JRST[	PUSHJ P,HEAD	;read hand also
00400			MOVE AC,HAND
00500			MOVEM AC,SET
00600			PUSHJ P,REFLEX
00700			PUSHJ P,READED]	;DOES NOT RETURN
00800		TRNN RUN
00900		JRST[	SKIPGE TAC,FUNCT	;not run, mabye a function
01000			JRST[	PUSHJ P,SETSET
01100				JRST NEXT]
01200			JUMPE TAC,[CAIL CBUF,BUF;try next buffer
01300				SETOM (CBUF)	;dont do this if first buffer
01400				MOVEM CBUF,CBUFS
01500				ADDI CBUF,1001
01600				CAILE CBUF,BUF2
01700				MOVEI CBUF,BUF		;Advance buffer
01800				MOVE 2,(CBUF)
01900				JUMPL 2,[BUFWT:MOVE CBUF,CBUFS;wait for buffer
02000					JRST SWDONE]
02100				JUMPN 2,BUFRDY		;the last buffer stop.
02200				JRST[	TRO DTST
02210					MOVE 2,STKPTR
02220					HRRM 2,JUMPBK
02230					MOVEI 2,WAITIN
02240					MOVEM 2,STKPTR
02300			IFN REPORT,<PUSH DATA,[0]>
02400					JRST BUFWT]
02500			BUFRDY:	HRRZ 2,(CBUF)
02600				ADDI 2,-1(CBUF)
02700				HRRZM 2,STKPTR
02800				JRST NEXT]
02900			CAIN TAC,6
03000			JRST SETCH
03100			JRST TT]	;otherwise perform function
03200		MOVEI J,6
03300		PUSHJ P,SERVO		;computes feedback and integral term
03400		JUMPE J,[RUDONE:TRZ DROP+RUN+FINAL+WOB+NUL	;if J is zero then end of run
03500			TDZ DATWD,[770001252500]
03600			PUSHJ P,SETSET
03700			TRZE STP
03800			JRST[	MOVEI TAC,STPERR
03900				MOVEM TAC,TRAJER
04000				POP P,TAC
04100				JRST TOFF]
04200			JRST NEXT]
04300		PUSHJ P,DRIVE		;Drive motors
04400		PUSHJ P,EVAL		;Compute TFF and TDD
04500		PUSHJ P,NXTH		;Compute next T0
     

00100	TT:	PUSHJ P,REFLEX
00200	DISP:	MOVE AC,FUNCT
00300		CAILE AC,MAXFN
00400		JRST NEXT
00500		JRST @.+1(AC)		;Perform function
00600		NXTJIF	;0
00700		HOPEN	;1
00800		HCLOSE	;2
00900		NXTJIF	;3
01000		PLACE	;4
01100		NUDGE	;5
01200		SETCH	;6
01300		STOP	;7
01400		SAVE	;10
01500		RESTORE	;11
01600		CENTER	;12
01700		SET.ARM	;13 SET_ARM
01800		WOBBLE	;14
01900		SEARCH	;15
02000		AOJI	;16
02100		SLAVE	;17
02200	
02300		XLIST
02400		SUBTTL	SPACEWAR CODE	ANGLE MEASUREMENT	LOU PAUL
02500		LIST
     

00100	ANGLES:	MOVEI I,5
00200		MOVE TAC,[XWD TH,THP]
00300		BLT TAC,THP+5
00400		MOVEI L,10
00500	IFN REPORT,<PUSH DATA,[<SIXBIT/THETA/>+6]>
00600	SETCHA:	MOVEI K,CHA
00700		TRNE ALT6
00800		MOVEI K,N6CHA
00900	RSTRT:	SOJL L,REDER
01000	IFE USER,<
01100		PUSHJ P,PREAD
01200		LDB AC,SNUM
01300		CAIN I,5
01400		JRST[	TRNE POTREAD
01500			JRST .+1
01600			CAIL AC,1000
01700			CAILE AC,7000
01800			JRST[	TRC ALT6
01900				JRST SETCHA]
02000			MOVEI K,CHA5
02100			JRST .+1]
02200		ANDI DACVAL,7777
02300		ADDI AC,(DACVAL)
02400		SETZ TAC,
02500		DIV AC,REFTR
02600		ASH AC,-26
02700		MOVE J,AC
02800		TRNE POTREAD
02900		JRST[	ADDI J,=1000
03000			JRST EXON]
03100		ASHC AC,-11		;LEAVE INDEX INTO NON-LINEAR DIFF TABLE IN AC
03200		LDB MQ,NON(I)		;GET LOWER DIFF
03300		ADDI J,(MQ)		;ADD IT TO READING
03400		AOJ AC,
03500		LDB AC,NON(I)		;GET UPPER DIFFERENCE
03600		SUBI AC,(MQ)
03700		MUL TAC,AC		;INTERPOLATE
03800		ADD J,TAC		;CORRECTED READING
03900		FSC J,226		;FLOAT
04000		FMPR J,SCALE(I)
04100		FADR J,OFF(I)
04200	>
04300	IFN USER,<
04400		MOVE J,ERR(I)
04500		FSC J,-1
04600		FADR J,T0(I)
04700	>
04800		CAIN I,5
04900		JRST[	TRNE ALT6
05000			FADR J,OFF6
05100			FADR J,ROT6
05200			MOVEM J,TH+5
05300			FSBR J,T0+5
05400			MOVSI MQ,(180.0)
05500			CAML J,MQ
05600			JRST[	INCROT:	MOVN MQ,MQ
05700					FSC MQ,1
05800					FADRM MQ,ROT6
05900					FADRM MQ,TH+5
06000					JRST THS]
06100			MOVN MQ,MQ
06200			CAMG J,MQ
06300			JRST INCROT
06400			JRST THS]
06500	EXON:	MOVEM J,TH(I)		;THETA
06600	THS:	CAILE I,2
06700		JRST[	MOVE AC,TH(I)
06800			FSBR AC,THP(I)
06900			FDVR AC,TDF
07000			MOVEM AC,TD(I)
07100			JRST .+1]
07200	IFN REPORT,<
07300		MOVE MQ,TH(I)
07400		FSBR MQ,T0(I)
07500		PUSH DATA,MQ
07600	>
07700	NXCHA:	SOJGE I,RSTRT	; IF LAST TIME THROUGH
     

00100		MOVEI I,2
00200	IFE USER,<
00300	NVEL:	PUSHJ P,PREAD
00400		LDB AC,SNUM
00500		ANDI DACVAL,7777
00600		ADDI AC,(DACVAL)
00700		TRNE VZERO
00800		MOVEM AC,VZ(I)
00900		SUB AC,VZ(I)
01000		FSC AC,216
01100		FMPR AC,VSCALE(I)
01200	IFN TACH,<
01300		TRNE VZERO
01400		SETZM VDTH(I)
01500		FADRM AC,VDTH(I)
01600	>
01700		MOVEM AC,TD(I)
01800		SOJGE I,NVEL
01900	>
02000		TRZ VZERO
02100		POPJ P,
02200	
02300		XLIST
02400				SUBTTL			SPACEWAR CODE			SERVO CALC.			LOU PAUL
02500		LIST
     

00100	SERVO:	MOVEI I,5
00200		SETZM ASTP
00300		MOVE AC,[XWD ET0,ET0P]
00400		BLT AC,ET0P+5
00500	SERVL:	MOVE TAC,TH(I)
00600		FSBR TAC,T0(I)		;POSITION ERROR
00700		MOVEM TAC,ET0(I)
00800		TRNE FINAL
00900		JRST NULL		;MODIFY THETA BY INTEGRAL TERM
01000	IFE STEP,<
01100		MOVN AC,T0(I)
01200		FSC AC,1
01300		FADR AC,TF(I)
01400		FADR AC,T0P(I)
01500	>
01600	IFN STEP,<SETZ AC,>
01700	MODT0:	MOVE MQ,CII(I)
01800		FMPR AC,MQ
01900		HRRZ K,BMASK(I)
02000		TDNN K,FBI
02100		JRST [	PDIR:SETZM ET0(I)
02200			JRST NOBACK]
02300		FMPR TAC,KI(I)
02400		FMPR MQ,MQ
02500		FDVR TAC,MQ
02600		FADRM TAC,ERRINT(I)
02700	IFE STEP,<
02800		MOVE TAC,T0P(I)
02900		FSBR TAC,T0(I)
03000		FADR TAC,TD(I)
03100	>
03200	IFN STEP,<MOVE TAC,TD(I)>
03300		MOVEM TAC,VERR(I)
03400	
03500		MOVN TAC,VERR(I)
03600		FMPR TAC,KV(I)		;VELOCITY GAIN
03700		FADR AC,TAC
03800		MOVN TAC,ET0(I)
03900		FMPR TAC,KE(I)		;ERROR GAIN
04000		FDVR TAC,CII(I)
04100		FADR AC,TAC
04200	
04300		FADR AC,ERRINT(I)
04400	
04500	NOBACK:	CAIE I,2
04600		FMPR AC,[IRAD:0.0174532925]	;JOINT 3 IS IN INCHES
04700		MOVEM AC,TORE(I)
04800		TRNE STP
04900		JRST[	MOVE MQ,AC
05000			FDVR MQ,F0(I)
05100			SKIPN TD(I)
05200			FSC MQ,-2
05300			FMPR MQ,STQ(I)
05400			FADRM MQ,ASTP
05500			JRST .+1]
05600		FADR AC,CI(I)		;GRAVITY TERM
05700		MOVEM AC,T(I)			;ERROR CORRECTION TORQUE
05800	NXTACC:
05900		SOJGE I,SERVL
06000		TRNE STP
06100		JRST[	MOVE AC,ASTP
06200			CAML AC,SDTQ
06300			JRST[	AOS AC,ASTPP
06400				CAIL AC,3
06500				JRST[	TRZ STP
06600					SUB P,[XWD 1,1]
06700					SETZM ERRINT
06800					MOVE AC,[XWD ERRINT, ERRINT+1]
06900					BLT AC,ERRINT+5
07000					JRST RUDONE]
07100				JRST .+1]
07200			SETZM ASTPP
07300			JRST .+1]
07400	IFN DYNAMICS,<
07500		HRRI TAC,1(DATA)
07600		HRLI TAC,DDAT
07700		HRRZI AC,15(TAC)
07800		BLT TAC,(AC)
07900		ADD DATA,[XWD 16,16]
08000	>
08100		POPJ P,
08200	
08300	NULL:	SETZ AC,
08400		TLNN DATWD,NNUL
08500		TRNE WOB
08600		SOJA J,MODT0
08700		TDNN DATWD,GMASK(I)
08800		SOJA J,NXTACC
08900		MOVE MQ,FBI
09000		TDNN MQ,GMASK(I)
09100		SOJA J,PDIR
09200		MOVM MQ,TAC			;|POSITION ERROR|
09300		CAML MQ,ERR(I)			;COMPARE POSITION ERROR
09400		JRST MODT0
09500		MOVM MQ,ET0P(I)
09600		CAML MQ,ERR(I)
09700		JRST MODT0
09800		MOVE MQ,ET0P(I)
09900		FMPR MQ,TAC
10000		JUMPL MQ,MODT0
10100	SO:	TRNE NUL
10200		SOJA J,MODT0
10300		TDZ DATWD,BMASK(I)
10400	IFN REPORT+TRACK+USER+DEB,<SETZM MOTOR(I)>
10500		DATAO ARM,DATWD
10600		HRLZ MQ,I
10700		TRC MQ,400000
10800		DATAO WIDTH,MQ
10900		SOJA J,NXTACC
11000		XLIST
11100				SUBTTL			SPACEWAR CODE			DRIVE CALC.			LOU PAUL
11200		LIST
     

00100	DRIVE:	JFCL 10,.+1
00200		MOVEI I,5		;Loop once for each joint
00300	IFN REPORT,<PUSH DATA,[<SIXBIT/DAC/>+6]>
00400		TRZ DATWD,504000
00500		TRO DATWD,021200		;INITIAL SIGN BITS FOR POSITIVE ROTATION
00600	LS0:	TDNN DATWD,BMASK(I)
00700		JRST ISP
00800		MOVE MQ,TD(I)
00900		JUMPN MQ,.+2
01000		MOVN MQ,ET0(I)
01100		FMPR MQ,T(I)
01200		CAIN I,1
01300		JRST[	HRRZ AC,FBI
01400			TDNN AC,BMASK(I)
01500			JRST .+1
01600			TRNE DROP
01700			JRST .+1
01800			SETZ MQ,
01900			JRST NOEX]
02000		JUMPL MQ,[	MOVM MQ,T(I)
02100				FSBR MQ,F0(I)
02200				JUMPL MQ,[	FDVR MQ,F0(I)
02300						FMPR MQ,V0(I)
02400						FSC MQ,1
02500						FADR MQ,V0(I)
02600						SKIPGE T(I)
02700						MOVN MQ,MQ
02800						JRST CEMF]
02900				MOVSI AC,(1.0)
03000				FSBR AC,PK(I)
03100				FMPR MQ,AC
03200				FMPR MQ,KM(I)
03300				FADR MQ,V0(I)
03400				SKIPG T(I)
03500				MOVN MQ,MQ
03600				JRST CEMF]
03700		SKIPN T(I)
03800		JRST[		SKIPN TD(I)
03900				JRST CEMF
04000				MOVE MQ,V0(I)
04100				SKIPGE TD(I)
04200				MOVN MQ,MQ
04300				JRST CEMF]
04400		JUMPE MQ,NOEX
04500		MOVE MQ,V0(I)
04600		SKIPGE T(I)
04700		MOVN MQ,MQ
04800	NOEX:	MOVSI AC,(1.0)
04900		FADR AC,PK(I)
05000		FMPR AC,T(I)
05100		FMPR AC,KM(I)
05200		FADR MQ,AC
05300	CEMF:
05400		MOVE AC,EMF(I)
05500		FMPR AC,TD(I)
05600		MOVSI J,(30.0)
05700		JUMPGE MQ,DRVLT
05800		MOVN AC,AC
05900	DRVLT:	FSBR J,AC			;Available drive voltage
06000		CAMGE J,[1.0]
06100		JRST MAXDRV
06200	VELOK:	MOVM AC,MQ
06300		FDVR AC,J		;AC= REL TIME ON
06400		FIX AC,211000		;1=16
06500		CAML AC,[4000000]
06600		JRST DHO		;too much force stop the arm
06700		CAILE AC,776000
06800	MAXDRV:	MOVEI AC,776000
06900	IFN REPORT+TRACK+USER+DEB,<
07000		MOVEM AC,MOTOR(I)
07100		SKIPGE MQ
07200		MOVNS MOTOR(I)
07300	>
07400		TRC AC,400000
07500		HRL AC,I
07600	SETDRV:
07700	IFN TRACK,<SKIPN WALK>
07800		DATAO WIDTH,AC
07900		JUMPGE MQ,ISP
08000		TDC DATWD,DMASK(I)
08100	ISP:
08200	IFN REPORT,<PUSH DATA,MOTOR(I)>
08300		SOJGE I,LS0
08400		JFCL 10,DHO
08500	IFN TRACK,<SKIPN WALK>
08600		DATAO ARM,DATWD
08700		POPJ P,
08800	
08900		XLIST
09000		SUBTTL	SPACEWAR CODE				LOU PAUL
09100		LIST
     

00100	NXTH:	MOVE AC,[XWD T0,T0P]
00200		BLT AC,T0+5
00300		TRNE WOB
00400		JRST[	MOVEI I,2
00500		WL1:	SOSGE J,WOBCNT(I)
00600			JRST[	MOVEI J,=19
00700				MOVEM J,WOBCNT(I)
00800				JRST WL2]
00900		WL2:	MOVE AC,SIN(J)
01000			FMPR AC,WOBMAG
01100			FADRM AC,TFF+3(I)
01200			SOJGE I,WL1
01300			JRST .+1]
01400		TRNE DROP
01500		JRST[	MOVEI I,5
01600		OL:	MOVE AC,DELTH(I)
01700			FMPR AC,TDF
01800			FADRM AC,DTH(I)
01900			SOJGE I,OL
02000			JRST JALS]
02100	NTH:
02200		TRNE INCREM
02300		JRST[	MOVE MQ,ETIME
02400			ADDB MQ,TICKS
02500			CAML MQ,NTICKS
02600			JRST[	TRZ INCREM
02700				SKIPN TP
02800				TRO FINAL
02900				MOVEI AC,=600
03000				MOVEM AC,COUNT
03100				MOVEI I,5
03200			FIL:	MOVE AC,DELTH(I)
03300				FADRB AC,DTH(I)
03400				FADR AC,TFF(I)
03500				MOVEM AC,TF(I)
03600				SETZM DELTH(I)
03700				SOJGE I,FIL
03800				POPJ P,]
03900			SETZ I,
04000			DIV MQ,NTICKS
04100			ASH MQ,-10
04200			FSC MQ,200
04300			MOVSI AC,(6.0)
04400			FMPR AC,MQ
04500			FADR AC,[-15.0]
04600			FMPR AC,MQ
04700			FADR AC,[10.0]
04800			FMPR AC,MQ
04900			FMPR MQ,MQ
05000			FMPR MQ,AC
05100			MOVEI I,5
05200		LDC:	MOVE AC,DELTH(I)
05300			FMPR AC,MQ
05400			FADR AC,DTH(I)
05500			FADR AC,TFF(I)
05600			MOVEM AC,TF(I)
05700			SOJGE I,LDC
05800			POPJ P,]
05900	JALS:	MOVEI I,5
06000	JAL:	MOVE AC,DTH(I)
06100		FADR AC,TFF(I)
06200		MOVEM AC,TF(I)
06300		SOJGE I,JAL
06400		POPJ P,
06500	
     

00100	EVAL:	MOVEI I,5
00200	EL:	MOVE J,TP(I)	;POINTS TO KOE5
00300		JUMPE J,NE
00400		AOS MQ,TJ(I)
00500		CAML MQ,TN(I)	;NUMBER OF TICKS THIS SEGMENT
00600		JRST[				;END OF SEGMENT
00700			CAIN I,5
00800			JRST[			;JOINT 6 HAS CONTROL WORD
00900				MOVE TAC,[XWD NC,CI]
01000				BLT TAC,CI+=11	;UPDATE C
01100				MOVE TAC,TICKS
01200				CAMGE TAC,NTICKS
01300				TRO INCREM
01400				LDB TAC,[POINT 3,1(J),2]	;LEVEL
01500				JUMPN TAC,[			;SWITCH POINT
01600					SKIPG LOOP(TAC)
01700					JRST[			;INITIALIZE LOOP
01800						LDB AC,[POINT 15,1(J),17]
01900						MOVEM AC,LOOP(TAC)	;COUNT
02000						JRST SETALT]
02100					SOSLE LOOP(TAC)		;IN LOOP
02200				SETALT:	TRO ALT
02300					JRST GETNXT]
02400				JRST GETNXT]
02500		GETNXT:	TRNE ALT
02600			JRST[	LDB J,[POINT 9,(J),8]	;ALT POINTER
02700				JRST GOT]
02800			LDB J,[POINT 9,(J),17]		;NEXT POINTER
02900			JUMPE J,[SETZM TP(I)
03000				TRO FINAL
03100				MOVEI AC,=600
03200				MOVEM AC,COUNT
03300				MOVE AC,A4(I)
03400				ADD AC,A3(I)
03500				ADD AC,A2(I)
03600				ADD AC,A1(I)
03700				ADD AC,A0(I)
03800				JRST ELL]
03900		GOT:	ADDI J,(CBUF)
04000			MOVEM J,TP(I)
04100			SETZM TJ(I)
04200			HRRZ AC,(J)
04300			MOVEM AC,TN(I)
04400			CAIN I,5
04500			JRST[
04600				HRRZ TAC,1(J)
04700				ADDI TAC,(CBUF)
04800				MOVEI MQ,14(TAC)
04900				HRLI TAC,(TAC)
05000				HRRI TAC,NC
05100				BLT TAC,NC+=11
05200				MOVEI K,5
05300			AAI:	MOVE TAC,MOTARM(K)
05400				FADRM TAC,NC+6(K)
05500				SOJGE K,AAI
05600				MOVEI K,5
05700				MOVE MQ,(MQ)
05800			FTF:	MOVE TAC,GMASK(K)
05900				TDNN TAC,FBI
06000				JRST[	TDNN TAC,MQ
06100					JRST FNF
06200					MOVE AC,TH(K)
06300					MOVEM AC,T0(K)
06400					FADR AC,TD(K)
06500					MOVEM AC,TF(K)
06600					FSBR AC,TFF(K)
06700					MOVEM AC,DTH(K)
06800					MOVNM AC,DELTH(K)
06900					MOVE AC,TN+5
07000					IMULI AC,=16667
07100					MOVEM AC,NTICKS
07200					SETZM TICKS
07300					TRO INCREM
07400					JRST FNF]
07500				TDNN TAC,MQ
07600				JRST[	MOVE AC,T0(K)
07700					FSC AC,1
07800					FSBR AC,T0P(K)
07900					FSBR AC,TF(K)
08000					CAIE K,2
08100					FMPR AC,IRAD
08200					FMPR AC,CII(K)
08300					FADR AC,T(K)
08400					MOVEM AC,CI(K)
08500					JRST FNF]
08600			FNF:	SOJGE K,FTF
08700				HRRZM MQ,FBI
08800				MOVEI K,=11
08900				MOVE AC,TN+5
09000				FSC AC,233
09100				MOVSI MQ,(1.0)
09200				FDVR MQ,AC
09300			DELC:	MOVE AC,NC(K)
09400				FSBR AC,CI(K)
09500				FMPR AC,MQ
09600				MOVEM AC,DCI(K)
09700				SOJGE K,DELC
09800				JRST DDELC]
09900		DDELC:	HLRE AC,-3(J)
10000			MOVEM AC,A4(I)
10100			HRRE AC,-3(J)
10200			MOVEM AC,A3(I)
10300			HLRE AC,-2(J)
10400			MOVEM AC,A2(I)
10500			HRRE AC,-2(J)
10600			MOVEM AC,A1(I)
10700			HLLZ AC,-1(J)
10800			MOVEM AC,SC(I)
10900			HRRE AC,-1(J)
11000			MOVEM AC,A0(I)
11100			JRST ELL]
11200		CAIN I,5
11300		JRST[	MOVEI K,=11
11400		DECC:	MOVE AC,DCI(K)
11500			FADRM AC,CI(K)
11600			SOJGE K,DECC
11700			JRST .+1]
11800		DIV MQ,TN(I)
11900		MOVE AC,A4(I)
12000		MUL AC,MQ
12100		ADD AC,A3(I)
12200		MUL AC,MQ
12300		ADD AC,A2(I)
12400		MUL AC,MQ
12500		ADD AC,A1(I)
12600		MUL AC,MQ
12700		ADD AC,A0(I)
12800	ELL:	XOR AC,SC(I)
12900		FSC AC,0
13000		MOVEM AC,TFF(I)
13100		CAIN I,5
13200		JRST[	MOVE AC,TFF+5
13300			FSBR AC,T0+5
13400			MOVSI MQ,(180.0)
13500			CAML AC,MQ
13600			JRST[	INCT0:	FSC MQ,1
13700					FADRM MQ,T0+5
13800					FADRM MQ,TF+5
13900					JRST .+1]
14000			MOVN MQ,MQ
14100			CAMG AC,MQ
14200			JRST INCT0
14300			JRST .+1]
14400	NE:	SOJGE I,EL
14500		TRZ ALT
14600		POPJ P,
14700	
     

00100	DHO:	AOJ I,			;DISASTER HAS OCCURED
00200		LSH I,3			;JOINT NUMBER
00300		IORI I,EXER
00400		MOVEM I,TRAJER
00500		POP P,I
00600	ZITO:	SETZM ERRINT
00700		MOVE AC,[XWD ERRINT,ERRINT+1]
00800		BLT AC,ERRINT+5
00900	
01000	TOFF:	MOVE AC,STKPTR
01100		MOVEM AC,ERRSTK
01200		MOVEI L,1000
01300		TRNE RONLY
01400		JRST READED
01500		TRZ RUN+FINAL+DROP+INCREM+HCL+STP+WOB+NUL	;STOP THE ARM
01600		SETZM TP
01700		MOVE AC,[XWD TP,TP+1]
01800		BLT AC,TP+5
01900		PUSHJ P,SETSET
02000	MOVNXT:	AOS AC,STKPTR
02100	MOVNX:	SOJL L,[ LE:
02200			MOVEI TAC,LOOPER
02300			MOVEM TAC,TRAJER
02400			JRST PW]
02500		HLRE TAC,@STKPTR
02600		CAIN TAC,3		;IS IT A WAIT
02700		JRST MOVNXT
02800		CAIN TAC,SKPERR
02900		JRST[	HRRZ TAC,(AC)
03000			CAMN TAC,TRAJER
03100			JRST[	AOS STKPTR
03200				JRST NEXT]
03300			JRST MOVNXT]
03400		CAIN TAC,SKPNER
03500		JRST[	HRRZ TAC,(AC)
03600			CAME TAC,TRAJER
03700			JRST[	AOS STKPTR
03800				JRST MOVNXT]
03900			JRST NEXT]
04000		CAIN TAC,SKPSER
04100		JRST[	LDB MQ,[POINT 3,(AC),35]
04200			LDB TAC,[POINT 3,TRAJER,35]
04300			CAIN TAC,(MQ)
04400			JRST[	HRRZ TAC,(AC)
04500				ANDI TAC,777770
04600				TDNE TAC,TRAJER
04700				JRST[	AOS STKPTR
04800					JRST NEXT]
04900				JRST MOVNXT]
05000			JRST MOVNXT]
05100		CAIN TAC,JMPREL
05200		JRST[	HRRE TAC,(AC)
05300			ADDI AC,(TAC)
05400			HRRM AC,STKPTR
05500			JRST MOVNX]
05600	PW:	CAIL AC,BUF
05700		HRRM AC,JUMPBK
05800		MOVEI AC,WAITIN
05900		MOVEM AC,STKPTR
06000		MOVEI AC,3
06100		MOVEM AC,FUNCT
06200	IFN TRACK,<SKIPN WALK>
06300		DATAO ARM,[2]		;STOP THE ARM
06400	IFN REPORT,<PUSH DATA,[0]>
06500	READED:	TRO DTST
06600	IFN DEB,<
06700		MOVEM 17,ACSAV+17
06800		MOVEI 17,ACSAV
06900		BLT 17,ACSAV+16
07000	>
07100		JRST SWDONE
07200	
     

00100	NEXT:	TRNE RUN
00200		JRST[	SETZM FUNCT
00300			JRST NXTJIF]
00400		MOVEI L,1000
00500		MOVE AC,FUNCT
00600		CAIE AC,6		;SET TOUCH
00700		JRST[	MOVE TAC,SOTCHS
00800			MOVEM TAC,SOTCH
00900			TRZ STCH
01000			JUMPE TAC,.+1
01100			TRO STCH
01200			JRST .+1]
01300	IFN TRACK,<SKIPN WALK>
01400		MOVEI DATWD,2
01500		DATAO ARM,DATWD		;STOP THE ARM
01600	INCPIC:	AOS AC,STKPTR
01700	PICKUP:	SOJL L,LE
01800		HLRE TAC,@STKPTR
01900		CAIN TAC,JMPABS
02000		JRST[JMPA:HRRZ TAC,@STKPTR
02100			MOVEM TAC,STKPTR
02110			HLRZ TAC,@STKPTR
02120			CAIN TAC,3
02130			JRST INCPIC
02200			JRST PICKUP]
02300		CAIN TAC,SKPERR
02400		JRST[	HRRZ TAC,@STKPTR
02500			CAMN TAC,TRAJER
02600			AOS STKPTR
02700			JRST INCPIC]
02800		CAIN TAC,SKPNER
02900		JRST[	HRRZ TAC,@STKPTR
03000			CAME TAC,TRAJER
03100			AOS STKPTR
03200			JRST INCPIC]
03300		CAIN TAC,SKPSER
03400		JRST[	LDB MQ,[POINT 3,@STKPTR,35]
03500			LDB TAC,[POINT 3,TRAJER,35]
03600			CAIN TAC,(MQ)
03700			JRST[	HRRZ TAC,@STKPTR
03800				ANDI TAC,777770
03900				TDNE TAC,TRAJER
04000				AOS STKPTR
04100				JRST INCPIC]
04200			JRST INCPIC]
04300		CAIN TAC,JMPREL
04400		JRST[	HRRE TAC,@STKPTR
04500			ADD TAC,STKPTR
04600			HRRM TAC,STKPTR
04700			JRST PICKUP]
04800		SETZM TRAJER
04810		MOVEI I,100
04820		MOVEM I,COUNT
04900		JUMPL TAC,[TRO RUN+VZERO+NUL
05000			SETCA TAC,
05100			MOVEM TAC,FUNCT
05200			AOS DOING		;INCREASE TRAJECTORY NUMBER
05300			HLRZ 2,(CBUF)
05400			IMULI 2,=16667
05500			MOVEM 2,NTICKS		;RUN TIME
05600			MOVEI 2,=1800
05700			MOVEM 2,COUNT
05800			SETZM LOOP
05900			MOVE TAC,[XWD LOOP,LOOP+1]
06000			BLT TAC,TN+5
06100			MOVE DATWD,777(CBUF)
06200			HRRZM DATWD,FBI
06300			MOVEI I,5
06400			MOVEI K,34
06500		SUL:	SUBI K,4
06600			HRLZM K,TJ(I)
06700			MOVEI TAC,TJ(I)
06800			MOVEM TAC,TP(I)
06900			TDNE DATWD,BMASK(I)
07000			TDO DATWD,BMASK(I)
07100			SETZM DTH(I)
07200			SOJGE I,SUL
07300	
07400			HRLZI TAC,763(CBUF)
07500			HRRI TAC,NC
07600			BLT TAC,NC+=11
07700			MOVEI I,5
07800		SAAI:	MOVE TAC,MOTARM(I)
07900			FADRM TAC,NC+6(I)
08000			SOJGE I,SAAI
08100			PUSHJ P,EVAL
08200			TRZ INCREM
08300			MOVEI I,5
08400		MOVBK:	MOVE AC,TH(I)
08500			MOVEM AC,T0(I)
08600			MOVEM AC,TF(I)
08700			FSBR AC,TFF(I)
08800			CAIN I,5
08900			JRST[	MOVSI MQ,(180.0)
09000				CAML AC,MQ
09100				JRST[SETROT:FSC MQ,1
09200					MOVN MQ,MQ
09300					FADRM MQ,TH+5
09400					FADRM MQ,ROT6
09500					JRST MOVBK]
09600				MOVN MQ,MQ
09700				CAMG AC,MQ
09800				JRST SETROT
09900				JRST ADDTF]
10000		ADDTF:	MOVEM AC,DTH(I)
10100			MOVN AC,AC
10200			FADRM AC,DELTH(I)
10300			SETZM TD(I)
10400			SOJGE I,MOVBK
10500			PUSHJ P,EVAL
10600			PUSHJ P,NXTH
10700			SETZM TICKS
10800			HLRE TAC,@STKPTR
10900			JRST MARK]
11000		MOVEM TAC,FUNCT
11100		CAIN TAC,3		;IS IT A WAIT
11110		JRST[	MOVE TAC,STKPTR
11300			CAIL TAC,BUF
11400			HRRM TAC,JUMPBK
11500			MOVEI TAC,WAITIN
11600			MOVEM TAC,STKPTR
11700	IFN REPORT,<PUSH DATA,[0]>
11800			TRO DTST
11900			PUSHJ P,SWDONE]
12000	MARK:
12100	IFN REPORT,<
12200		PUSH DATA,[<SIXBIT/NEXT/>+1]
12300		MOVE TAC,FUNCT
12400		HRL TAC,0
12500		PUSH DATA,TAC
12600	>
12700	
12800	;	JRST DISP
12900	SWDONE:
13000		MOVEM BITSAV
13100		MOVEM DATWD,DATWDS
13200		MOVEM CBUF,CBUFS
13300	IFN REPORT+TRACK,<
13400		MOVEM DATA,DATPT
13500	>
13600	IFE USER,<
13700	IFN TRACK,<
13800		SKIPE WALK
13900		JRST @STRADR-1
14000	>
14100	;	CONO 4,200		;TURN ON THE PI'S
14200		CALLI 400024
14300	>
14400	IFN USER,<
14500		JRST @STRADR-1
14600	>
14700	
14800	SETSET:	MOVEI I,5
14900	TCHK:	MOVE J,TP(I)
15000		JUMPE J,ATEND
15100		SETZM TJ(I)
15200		SETZM TN(I)
15300	GNN:	LDB J,[POINT 9,(J),17]
15400		ADDI J,(CBUF)
15500		CAIN J,(CBUF)
15600		JRST[	MOVEM J,TP(I)
15700			JRST ATEND]
15800		JRST GNN
15900	ATEND:	SOJGE I,TCHK
16000		PUSHJ P,EVAL
16100		MOVEI I,5
16200	FDTH:	MOVE AC,TH(I)
16300		MOVEM AC,T0(I)
16400		MOVEM AC,T0P(I)
16500		MOVEM AC,TF(I)
16600		FSBR AC,TFF(I)
16700		MOVEM AC,DTH(I)
16800		SETZM DELTH(I)
16900		SOJGE I,FDTH
17000		POPJ P,
17100	
     

00100	PREAD:
00200	IFN TRACK,<
00300		SKIPE WALK
00400		JRST[GNPR:AOBJP DATA,[	MOVE AC,[IOWD DBL,DATO]
00500					MOVEM AC,DATPT
00600					MOVE DATA,DATPT
00700					SETZ TAC,
00800					INPUT 15,1
00900					JRST GNPR]
01000			MOVE DACVAL,(DATA)
01100			JRST @READFN]
01200	>
01300		MOVEI TAC,10
01400		MOVEM TAC,DATMSD
01500	DACRED:	SOSGE DATMSD
01600		JRST[REDER:	MOVEI TAC,READER	
01700				MOVEM TAC,TRAJER
01800				JRST TOFF]
01900	DACST:	CONO DB,4250
02000		CONO AD,(K)
02100	NREAD:	MOVEI TAC,30
02200		CONSO DB,1000
02300		SOJGE TAC,.-1
02400		JUMPL TAC,[DACDIE:CONO AD,4000(K)	;STOP DAC
02500				TRO REREAD
02600			IFN DEB,<MOVEM DACVAL,DACERR>
02700				MOVEI TAC,12
02800			WRH:	SOJGE TAC,WRH
02900				JRST DACRED]
03000		DATAI DB,DACVAL
03100		CONSZ DB,10000
03200		JRST DACDIE
03300		XOR DACVAL,SBMSK
03400	IFN TRACK,<PUSH DATA,DACVAL>
03500		JRST @READFN
03600	FREAD:	ADDI K,770000
03700	IFN TRACK,<SKIPN WALK>
03800		CONO AD,4000(K)	;STOP AD ON NEXT CHANNEL
03900		POPJ P,
04000	READFN:	FREAD
04100	
04200		XLIST
04300		SUBTTL	CONSTANTS				LOU PAUL
04400		LIST
     

00100	NON:	POINT 12,INNER(AC),11
00200		POINT 12,INNER(AC),23
00300		POINT 12,INNER(AC),35
00400		POINT 12,OUTER(AC),11
00500		POINT 12,OUTER(AC),23
00600		POINT 12,OUTER(AC),35
00700	ERR:	0.05
00800		0.05
00900		0.01
01000		0.1
01100		0.1
01200		0.7
01300	TH:	BLOCK 6		;THE NEXT 7 LOCATIONS ARE BLOCK TRANSFERED.
01400	HAND:	3.0
01500	VERR:	BLOCK 6
01600	ERRINT:	0
01700		0
01800		0
01900		0
02000		0
02100		0
02200	DTH:	BLOCK 6
02300	TD:	BLOCK 6
02400	ET0P:	BLOCK 6
02500	ET0:	BLOCK 6
02600	T0P:	BLOCK 6
02700	T0:	-150.0
02800		-75.0
02900		14.0
03000		-90.0
03100		 60.0
03200		 0.0000
03300	TF:	BLOCK 6
03400	TFF:	-150.0
03500		-75.0
03600		14.0
03700		-90.0
03800		 60.0
03900		 0.0000
04000	DELTH:	BLOCK 6		;THIS AND THE NEXT ARE BLT ED TOGETHER
04100	NTICKS:	0
04200	DTHS:	BLOCK =30
04300	TORE:	BLOCK 6
04400	T:	BLOCK 6
04500	IFN USER+REPORT+TRACK+DEB,<MOTOR:	BLOCK 6>
04600	FBI:	0
04700	VZ:	BLOCK 3
04800	IFN TACH,<
04900	VDTH:	BLOCK 3
05000	>
05100	THP:	BLOCK 6
05200	MOTARM:	700000.0
05300		800000.0
05400		5000.0
05500		3000.0
05600		3000.0
05700		4000.0
05800	STQ:	BLOCK 6
05900	SDTQ:	0
06000	ASTP:	0
06100	ASTPP:	0
06200	DCI:	BLOCK 6
06300	DCII:	BLOCK 6
06400	CI:	0
06500		0
06600		0
06700		0
06800		0
06900		0
07000	CII:	500000.0
07100		700000.0
07200		 10000.0
07300		100000.0
07400		100000.0
07500		  1000.0
07600	NC:	BLOCK =12
07700	IFN DEB,<DACERR:	0>
07800	REFTR:	0
07900	IFN DEB,<
08000	ETIM:	0
08100	ACSAV:	BLOCK 20
08200	>
08300	PDL:	DHO
08400		BLOCK PDLL
08500	LTIME:	0
08600	FUNCT:	0
08700	ROT6:	0
08800	TRAJER:	0
08900	DOING:	0
09000	DATMSD:	0
09100	CBUFS:	0
09200	DATWDS:	0
09300	SNUM:	POINT 12,DACVAL,23
09400	SBMSK:	400040004000
09500	DMASK:	400000
09600		100000
09700		20000
09800		4000
09900		1000
10000		200
10100	BMASK:	XWD 400000,200000
10200		XWD 200000, 40000
10300		XWD 100000, 10000
10400		XWD  40000,  2000
10500		XWD  20000,   400
10600		XWD  10000,   100
10700	GMASK:	200000
10800		 40000
10900		 10000
11000		  2000
11100		   400
11200		   100
11300	GAIN:	MOVEI I,5
11400	SETGN:	MOVE AC,KV(I)
11500		FMPR AC,AC
11600		FSC AC,-2
11700		MOVEM AC,KE(I)
11800		FMPR AC,KV(I)
11900		FSC AC,-4
12000		MOVNM AC,KI(I)
12100		SOJGE I,SETGN
12200		MOVE 1,[SIXBIT/$ARM$/]
12300		CALLI 1,400002
12400		MOVE I,RET
12500		MOVEM I,GAIN
12600		JRST GAIN
12700	RET:	POPJ P,
12800		BLOCK 11
12850		0
12900	WAITIN:	XWD 3,0
13000	JUMPBK:	XWD JMPABS,WAITIN
13100	STKPTR:	WAITIN
13200	ERRSTK:	0
13300	BITSAV:	0
13400	COUNT:	0
13500	STWT:	0
13600	HCNT:	0
13700	TP:	0
13800		0
13900		0
14000		0
14100		0
14200		0
14300	TJ:	BLOCK 6
14400	LOOP:	BLOCK 10
14500	TN:	BLOCK 6
14600	SC:	BLOCK 6
14700	A0:	BLOCK 6
14800	A1:	BLOCK 6
14900	A2:	BLOCK 6
15000	A3:	BLOCK 6
15100	A4:	BLOCK 6
15200	BLOCKS:	0
15300	NXTBUF:	0
15400	BUF:	0
15500		BLOCK 1000
15600	BUF2:	0
15700		BLOCK 1000
15800	IFN ADJUST<
15900	ADJI:	-1
16000	ADJE:	0
16100	ADJV:	0
16200	ADJIF:	0
16300	FACTOR:	BLOCK 3
16400	>
16500	IFN REPORT+TRACK,<
16600	DATIWD:	0
16700		0
16800	DATPT:	0
16900	DATO:	BLOCK DBL+100
17000	>
17100	IFN DYNAMICS,<
17200	DDAT:	<SIXBIT/FORD/>+6
17300	FOR:	BLOCK 6
17400		<SIXBIT/BACK/>+6
17500	BAK:	BLOCK 6
17600	>